#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

static const std::string WINDOW_COLOR = "Color View Window";
static const std::string WINDOW_DEPTH = "Depth View Window";

class ImageProcessor
{
  ros::NodeHandle node_handle_;
  image_transport::ImageTransport img_transport_;
  image_transport::Subscriber sub_color_image_;
  image_transport::Subscriber sub_depth_image_;
  image_transport::Publisher pub_color_image_;
  image_transport::Publisher pub_depth_image_;

public:
  ImageProcessor()
    : img_transport_(node_handle_)
  {
    // Subscribe to the image topics and publish the output images
    sub_color_image_ = img_transport_.subscribe("/camera/color/image_raw", 1, &ImageProcessor::processColorImage, this);
    sub_depth_image_ = img_transport_.subscribe("/camera/depth/image_rect_raw", 1, &ImageProcessor::processDepthImage, this);
    pub_color_image_ = img_transport_.advertise("/image_processor/output/color", 1);
    pub_depth_image_ = img_transport_.advertise("/image_processor/output/depth", 1);

    // Create OpenCV windows for visualization
    cv::namedWindow(WINDOW_COLOR);
    cv::namedWindow(WINDOW_DEPTH);
  }

  ~ImageProcessor()
  {
    // Destroy OpenCV windows when the object is destroyed
    cv::destroyWindow(WINDOW_COLOR);
    cv::destroyWindow(WINDOW_DEPTH);
  }

  void processColorImage(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr cv_image_ptr;
    try
    {
      cv_image_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("Error in color image processing: %s", e.what());
      return;
    }

    // Apply a graphical overlay to the image (e.g., a circle)
    if (cv_image_ptr->image.rows > 60 && cv_image_ptr->image.cols > 60)
      cv::circle(cv_image_ptr->image, cv::Point(50, 50), 10, CV_RGB(255, 0, 0));

    // Display the processed image
    cv::imshow(WINDOW_COLOR, cv_image_ptr->image);
    cv::waitKey(3);

    // Publish the modified image
    pub_color_image_.publish(cv_image_ptr->toImageMsg());
  }

  void processDepthImage(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr cv_image_ptr;
    try
    {
      cv_image_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("Error in depth image processing: %s", e.what());
      return;
    }

    // Apply a graphical overlay to the depth image (e.g., a circle)
    if (cv_image_ptr->image.rows > 60 && cv_image_ptr->image.cols > 60)
      cv::circle(cv_image_ptr->image, cv::Point(50, 50), 10, CV_RGB(255, 0, 0));

    // Display the processed depth image
    cv::imshow(WINDOW_DEPTH, cv_image_ptr->image);
    cv::waitKey(3);

    // Publish the modified depth image
    pub_depth_image_.publish(cv_image_ptr->toImageMsg());
  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_processor");
  ImageProcessor img_processor;
  ros::spin();
  return 0;
}

